/**
* @brief 统一驱动: 基于温度传感器反馈的散热风扇恒温控制 PID 驱动
 * @author 张勇 41204@qq.com
 * @date 2022-04
*/

#include "temp_fan_pid.h"

// PID 计算函数
static float _pid_compute(unidrv_temp_fan_pid_t *pid, float input) {
    // 计算误差
    float error = pid->setpoint - input;    
    
    // 比例项
    float proportional = pid->Kp * error;    
    
    // 积分项(带抗饱和)
    pid->integral += error;
    
    // 积分限幅
    if (pid->integral > pid->output_max) pid->integral = pid->output_max;
    if (pid->integral < pid->output_min) pid->integral = pid->output_min;
    float integral = pid->Ki * pid->integral;
    
    // 微分项
    float derivative = pid->Kd * (error - pid->prev_error);
    pid->prev_error = error;
    
    // 计算总输出
    float output = proportional + integral + derivative;
    
    // 输出限幅
    if (output > pid->output_max) output = pid->output_max;
    if (output < pid->output_min) output = pid->output_min;
    
    return output;
}

static void _pid_loop(size_t len, void* data) {
	if(len < sizeof(void*) || !data) { return; }
	unidrv_temp_fan_pid_t *pid = (unidrv_temp_fan_pid_t*)mcu_get_pointer(data);
    
    // 获取当前温度
    float current_temp = pid->get_temperature();
    
    // 计算PID输出
    float output = _pid_compute(pid, current_temp);
    
    // 设置风扇转速(0-100)
    pid->set_fan_speed(output);
    
    // 可以在这里添加调试输出
    // printf("Temp: %.2f°C, Fan: %d%%, Error: %.2f\n", 
    //        current_temp, (int)output, pid->setpoint - current_temp);
}

std_err_t unidrv_temp_fan_pid_start(unidrv_temp_fan_pid_t *pid, float Kp, float Ki, float Kd, float target_temperature, uint32_t interval_ms) {
    if(!pid) return STD_ERR_INVALID_ARG;
    
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->setpoint = target_temperature;
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->output_min = 0.0f;
    pid->output_max = 100.0f;
    
    if(!pid->pidtask) {
        pid->pidtask = unios_task_run_every(interval_ms, UNIOS_TASK_REPEAT_FOREVER, _pid_loop, sizeof(pid), &pid, false);
    }
    return pid->pidtask ? STD_ERR_OK: STD_ERR_FAIL;
}
